/**
 ****************************************************************************************************
 * @file        foc_pwm.c
 * @author      哔哩哔哩-Rebron大侠
 * @version     V0.0
 * @date        2025-01-11
 * @brief       SVPWM实现
 * @license     MIT License
 *              Copyright (c) 2025 Reborn大侠
 *              允许任何人使用、复制、修改和分发该代码，但需保留此版权声明。
 ****************************************************************************************************
 */
 
#include "foc_pwm.h"
#include "foc_port.h"
#include "foc_math.h"

/*扇区判断*/
static uint8_t Sector_Judgment(FOC_PWM_t *foc_pwm, alphabeta_t alphabeta, uint8_t *sector)
{
    foc_pwm->svpwm_val1 = alphabeta.alpha *SQRT_3_DIV_2;
    foc_pwm->svpwm_val2 = alphabeta.beta/2.0f;

    float A = alphabeta.beta;
    float B = foc_pwm->svpwm_val1 - foc_pwm->svpwm_val2;
    float C = -foc_pwm->svpwm_val1 - foc_pwm->svpwm_val2;

    uint8_t N = 0;
    if(A>0){
        N += 1;
    }

    if(B>0){
        N += 2;
    }

    if(C>0){
        N += 4;
    }
#if 1    /*进行转换 将N 转换成 Ⅰ Ⅱ Ⅲ Ⅳ Ⅴ Ⅵ */ 
    switch(N){
        case 3:
            *sector = 1;
            break;
        case 1:
            *sector = 2;
            break;
        case 5:
            *sector = 3;
            break;
        case 4:
            *sector = 4;
            break;
        case 6:
            *sector = 5;
            break;
        case 2:
            *sector = 6;
            break;
    }
#endif
    return N;
}

/*矢量时间计算*/
static void VectorActionTime(FOC_PWM_t *foc_pwm, uint8_t sector, alphabeta_t alphabeta, float Tpwm, float Udc)
{
    float K = Tpwm * SQRT_3/Udc;

    float X = K *(alphabeta.beta);
    float Y = K *(foc_pwm->svpwm_val1 + foc_pwm->svpwm_val2);
    float Z = K *(-foc_pwm->svpwm_val1 + foc_pwm->svpwm_val2);

    float T4 = 0, T6 = 0;
    float Ta = 0, Tb = 0, Tc = 0;
    float T1 = 0, T2 = 0, T3 = 0;

    switch(sector){
        case 1:
            T4 = Z;
            T6 = Y;
            break;
        case 2:
            T4 = Y;
            T6 = -X;
            break;
        case 3:
            T4 = -Z;
            T6 = X;
            break;
        case 4:
            T4 = -X;
            T6 = Z;
            break;
        case 5:
            T4 = X;
            T6 = -Y;
            break;
        case 6:
            T4 = -Y;
            T6 = -Z;
            break;
    }

    if(T4+T6 > Tpwm){
        T4 = T4/(T4+T6) *Tpwm;
        T6 = T6/(T4+T6) *Tpwm;
    }

    Ta = (Tpwm-T4-T6)/4.0f;
    Tb = Ta + T4/2.0f;
    Tc = Tb + T6/2.0f;

    switch(sector){
        case 1:
            T1 = Tb;
            T2 = Ta;
            T3 = Tc;
            break;
        case 2:
            T1 = Ta;
            T2 = Tc;
            T3 = Tb;
            break;
        case 3:
            T1 = Ta;
            T2 = Tb;
            T3 = Tc;
            break;
        case 4:
            T1 = Tc;
            T2 = Tb;
            T3 = Ta;
            break;
        case 5:
            T1 = Tc;
            T2 = Ta;
            T3 = Tb;
            break;
        case 6:
            T1 = Tb;
            T2 = Tc;
            T3 = Ta;
            break;
    }

    foc_pwm->T_abc.a = T1;
    foc_pwm->T_abc.b = T2;
    foc_pwm->T_abc.c = T3;
    

    U_H_SET_PWM(T1);
    V_H_SET_PWM(T2);
    W_H_SET_PWM(T3);
}

/*输出svpwm*/
static void FOC_Set_SVPWM(FOC_PWM_t *foc_pwm, qd_t U_qd, float angle_el)
{
    uint8_t N;
    alphabeta_t input = {0};

    input = FOC_Rev_Park(U_qd, angle_el);
    foc_pwm->alpha_beta = input;
    N = Sector_Judgment(foc_pwm, input, &foc_pwm->sector);
    VectorActionTime(foc_pwm, N, input, foc_pwm->Tpwm, foc_pwm->power*2.0f/3.0f);
}

/*运行pwm*/
void FOC_PWM_Run(FOC_PWM_t *foc_pwm, qd_t Uqd, float ElAngle)
{
    foc_pwm->Uqd = Uqd;
    
    FOC_Set_SVPWM(foc_pwm, Uqd, ElAngle);
}

void FOC_PWM_Init(FOC_PWM_t *foc_pwm, uint32_t Tpwm, float power)
{
    foc_pwm->Tpwm = Tpwm;
    foc_pwm->power = power;

    /*开启定时器的周期中断*/
    HAL_TIM_Base_Start_IT(&htim1);

    /*输出PWM比较中断*/
    HAL_TIM_OC_Start_IT(&htim1, TIM_CHANNEL_4);
}




